using LightCAD.MathLib;
using System;
using System.Collections;
using System.Collections.Generic;

namespace LightCAD.Three
{

    public class CatmullRomCurve3 : Curve<Vector3>
    {

        private class CubicPoly
        {
            private double c0 = 0;
            private double c1 = 0;
            private double c2 = 0;
            private double c3 = 0;

            public CubicPoly()
            {
            }

            /*
             * Compute coefficients for a cubic polynomial
             *   p(s) = c0 + c1*s + c2*s^2 + c3*s^3
             * such that
             *   p(0) = x0, p(1) = x1
             *  and
             *   p'(0) = t0, p'(1) = t1.
             */
            private void init(double x0, double x1, double t0, double t1)
            {

                c0 = x0;
                c1 = t0;
                c2 = -3 * x0 + 3 * x1 - 2 * t0 - t1;
                c3 = 2 * x0 - 2 * x1 + t0 + t1;

            }


            public void initCatmullRom(double x0, double x1, double x2, double x3, double tension)
            {

                init(x1, x2, tension * (x2 - x0), tension * (x3 - x1));

            }

            public void initNonuniformCatmullRom(double x0, double x1, double x2, double x3, double dt0, double dt1, double dt2)
            {

                // compute tangents when parameterized in [t1,t2]
                var t1 = (x1 - x0) / dt0 - (x2 - x0) / (dt0 + dt1) + (x2 - x1) / dt1;
                var t2 = (x2 - x1) / dt1 - (x3 - x1) / (dt1 + dt2) + (x3 - x2) / dt2;

                // rescale tangents for parametrization in [0,1]
                t1 *= dt1;
                t2 *= dt1;

                init(x1, x2, t1, t2);

            }

            public double calc(double t)
            {

                var t2 = t * t;
                var t3 = t2 * t;
                return c0 + c1 * t + c2 * t2 + c3 * t3;

            }

        }


        #region scope properties or methods
        private static Vector3 tmp = /*@__PURE__*/ new Vector3();
        private static CubicPoly px = /*@__PURE__*/ new CubicPoly();
        private static CubicPoly py = /*@__PURE__*/ new CubicPoly();
        private static CubicPoly pz = /*@__PURE__*/ new CubicPoly();
        #endregion

        #region Properties

        public ListEx<Vector3> points;
        public bool closed;
        public string curveType;
        public double tension;

        #endregion

        #region constructor
        public CatmullRomCurve3(ListEx<Vector3> points = null, bool closed = false, string curveType = "centripetal", double tension = 0.5)
        {
            if (points == null) points = new ListEx<Vector3>();
            this.type = "CatmullRomCurve3";
            this.points = points;
            this.closed = closed;
            this.curveType = curveType;
            this.tension = tension;
        }
        #endregion

        #region methods
        public override Vector3 getPoint(double t, Vector3 optionalTarget = null)
        {
            if (optionalTarget == null) optionalTarget = new Vector3();
            var point = optionalTarget;
            var points = this.points;
            var l = points.Length;
            var p = (l - (this.closed ? 0 : 1)) * t;
            var intPoint = (int)Math.Floor(p);
            var weight = p - intPoint;
            if (this.closed)
            {
                intPoint += intPoint > 0 ? 0 : (int)(Math.Floor((double)Math.Abs(intPoint) / l) + 1) * l;
            }
            else if (weight == 0 && intPoint == l - 1)
            {
                intPoint = l - 2;
                weight = 1;
            }
            Vector3 p0, p3; // 4 points (p1 & p2 defined below)
            if (this.closed || intPoint > 0)
            {
                p0 = points[(intPoint - 1) % l];
            }
            else
            {
                // extrapolate first point
                tmp.SubVectors(points[0], points[1]).Add(points[0]);
                p0 = tmp;
            }
            var p1 = points[intPoint % l];
            var p2 = points[(intPoint + 1) % l];
            if (this.closed || intPoint + 2 < l)
            {
                p3 = points[(intPoint + 2) % l];
            }
            else
            {
                // extrapolate last point
                tmp.SubVectors(points[l - 1], points[l - 2]).Add(points[l - 1]);
                p3 = tmp;
            }
            if (this.curveType == "centripetal" || this.curveType == "chordal")
            {
                // init Centripetal / Chordal Catmull-Rom
                var pow = this.curveType == "chordal" ? 0.5 : 0.25;
                var dt1 = Math.Pow(p1.DistanceToSquared(p2), pow);
                var dt2 = Math.Pow(p2.DistanceToSquared(p3), pow);
                var dt0 = Math.Pow(p0.DistanceToSquared(p1), pow);
                // safety check for repeated points
                if (dt1 < 1e-4) dt1 = 1.0;
                if (dt0 < 1e-4) dt0 = dt1;
                if (dt2 < 1e-4) dt2 = dt1;
                px.initNonuniformCatmullRom(p0.X, p1.X, p2.X, p3.X, dt0, dt1, dt2);
                py.initNonuniformCatmullRom(p0.Y, p1.Y, p2.Y, p3.Y, dt0, dt1, dt2);
                pz.initNonuniformCatmullRom(p0.Z, p1.Z, p2.Z, p3.Z, dt0, dt1, dt2);
            }
            else if (this.curveType == "catmullrom")
            {
                px.initCatmullRom(p0.X, p1.X, p2.X, p3.X, this.tension);
                py.initCatmullRom(p0.Y, p1.Y, p2.Y, p3.Y, this.tension);
                pz.initCatmullRom(p0.Z, p1.Z, p2.Z, p3.Z, this.tension);
            }
            point.Set(
                px.calc(weight),
                py.calc(weight),
                pz.calc(weight)
            );
            return point;
        }
        public override Curve<Vector3> copy(Curve<Vector3> source)
        {
            return copy(source as CatmullRomCurve3);
        }
        public CatmullRomCurve3 copy(CatmullRomCurve3 source)
        {
            base.copy(source);
            this.points = new ListEx<Vector3>();
            for (int i = 0, l = source.points.Length; i < l; i++)
            {
                var point = source.points[i];
                this.points.Push(point.Clone());
            }
            this.closed = source.closed;
            this.curveType = source.curveType;
            this.tension = source.tension;
            return this;
        }

        public override Curve<Vector3> clone()
        {
            return new CatmullRomCurve3().copy(this);
        }

        #endregion

    }
}
